%% Robot localization
function [r,theta,v,P_cur,R_cur,x_hat_cur] = localize_robot(r_ref,v_ref,x_hat_old,cur_time,omega,P,theta_ref,Kalman_param,R_old,flag_Kalman,loop_ind) 
global record_data error

% read odometry data from robot
odometry = pioneer_read_odometry();
r_x = odometry(1); 
r_y = odometry(2); 
theta = odometry(3);

r_x = r_x/1e3;
r_y = r_y/1e3;
theta = theta*pi/2048;
disp('odometry delta theta');
disp(radtodeg(theta));
% check if odometry angle is ok
if (theta>2*pi)||(theta<0)
    error = 1;
end

if flag_Kalman
    [x_hat_cur,z,P_cur,R_cur] = Kalman_filter(r_x,r_y,theta,r_ref,v_ref,x_hat_old,cur_time,omega,P,theta_ref,Kalman_param,R_old);
else
    P_cur = P; R_cur = R_old; x_hat_cur = x_hat_old;
end

% get new position, orientation and velocity
if flag_Kalman     
        record_data.r_odometry(loop_ind,:) = zeros(1,2);
        record_data.r_odometry(loop_ind,:) = z(1:2)';
        record_data.theta_odometry(loop_ind) = z(3);
        r = x_hat_cur(1:2)';
        theta = x_hat_cur(3);
        v = [ cos(theta) sin(theta) ]*v_ref;
else
        % measurement
        z = [ (([ cos(-theta_ref) sin(-theta_ref); -sin(-theta_ref) cos(-theta_ref) ]*[ r_x r_y ]')' + r_ref)'; theta + theta_ref ];
        if z(3) > pi 
            z(3) = z(3) - 2*pi;
        end
    
        r = z(1:2)';
        theta = z(3);
        v = [ cos(theta) sin(theta) ]*v_ref;    
end
